#!/usr/bin/env python3

import hybrid_astar as ha
from Parameter import C as PC
import math


def aStarPretreatment(startPoint,globalPoint):
    """
    终末点预处理
    :param startPoint: 起点
    :param globalPoint: 终点
    """
    startPoint[0] = startPoint[0]  / PC.AStar.N + PC.AStar.MAP_SIZE[0] / 2
    startPoint[1] = startPoint[1] / PC.AStar.N + PC.AStar.MAP_SIZE[1] / 2
    globalPoint[0] = globalPoint[0]  / PC.AStar.N + PC.AStar.MAP_SIZE[0] / 2
    globalPoint[1] = globalPoint[1] / PC.AStar.N + PC.AStar.MAP_SIZE[1] / 2

def aStarPostProcessing(path):
    """
    终末点后处理
    :param path: 轨迹
    """
    for i in range(len(path.x)):
        path.x[i] = (path.x[i] - PC.AStar.MAP_SIZE[0]/2) * PC.AStar.N
        path.y[i] = (path.y[i] - PC.AStar.MAP_SIZE[1]/2) * PC.AStar.N

def aStarTrajectoryPlanning(startPoint,globalPoint,showPlot = True):
    """
    astar 轨迹规划
    :param startPoint: 起点[x,y,yaw]
    :param globalPoint: 终点[x,y,yaw]
    :param showPlot: 是否显示轨迹
    """
    aStarPretreatment(startPoint,globalPoint)
    path = ha.main(startPoint, globalPoint, showPlot)
    aStarPostProcessing(path)
    return path

if __name__ == '__main__':
    startPoint = [0,0,math.radians(45)]
    globalPoint = [1,1,math.radians(120)]
    path = aStarTrajectoryPlanning(startPoint,globalPoint)